
#include "serialization/asn1.h"
#include "node_sidecar.h"

#include "Debug.h"
#include "Log.h"

int router_queue_push_inf(t1_inf_t* inf, uint16_t len);

static int node_sidecar_platforminfo(void) {
	LOG_I("sidecar platforminfo");
    asn1_builder_t build_sidecar_response_platform_info;
    ASN1(build_sidecar_response_platform_info, ->TAG(PAYLOAD_RESPONSE)
                                             ->TAG(SIDECAR_RESPONSE)
                                             ->TAG(SIDECAR_RESPONSE_PLATFORM_INFO)
                                             ->VALUE({0x00})
                                             ->CLOSEALL());

    t1_inf_t inf = {.header = {.destination = NODE_SAM,
                               .source = NODE_BLE,
                               .reply = NODE_NULL,
                               .secure_channel = 0,
                               .RFU1 = 0,
                               .RFU2 = 0},
                    .payload = build_sidecar_response_platform_info.payload};
	router_queue_push_inf(&inf, build_sidecar_response_platform_info.dataPtr);
	return 0;
}

typedef enum { sidecarModuleCommandPlatformInfo = 0, TOTAL_COMMANDS } sidecarModuleCommandId_t;

typedef int (*command_handler)(void);

typedef struct {
    sidecarModuleCommandId_t sidecarModuleCommandId;
    command_handler handler;
} sidecarModuleCommands_t;
sidecarModuleCommands_t const sidecarModuleCommands[] = {
    [sidecarModuleCommandPlatformInfo] = {0x03, node_sidecar_platforminfo}};

int node_sidecar_handler(t1_inf_t *inf, uint16_t len) {
    asn1_tlv_t command = asn1_unwrap(inf->payload);

    if (command.tag.tagid == sidecarCommand) {
        asn1_tlv_t sidecarCommand = asn1_unwrap(command.value);
        for (sidecarModuleCommandId_t i = 0; i < TOTAL_COMMANDS; i++) {
            if (sidecarModuleCommands[i].sidecarModuleCommandId == sidecarCommand.tag.tagid) {
				return sidecarModuleCommands[i].handler();
            }
        }
        LOG_I("No handler for command %02X", sidecarCommand.tag.tagid);
    }
	
	return 2;
}

